#include "match_projection_point.h"

Match_Projection_Point::Match_Projection_Point()
{
}

Match_Projection_Point::~Match_Projection_Point(){
}

PointAndIndex Match_Projection_Point::match_and_projection_point(const std::pair<double, double>& odom_receive_, 
                                                                                                const std::vector<std::pair<double, double>>& points){

    std::pair<double, double> closest_point_;
    std::pair<double, double> current_odom = odom_receive_;
    std::vector<std::pair<double, double>> xy_points = points;
    double min_distance = std::numeric_limits<double>::max();
    int potential_closest_index = last_closest_index < 0 ? 0 : last_closest_index;

    // 第一次搜索或从上次最近点开始搜索
    if(last_closest_index < 0){
        for (int i = 0; i <= xy_points.size() - 1; ++i) {
            double distance = std::sqrt(std::pow(xy_points[i].first - current_odom.first, 2) + 
                                                                    std::pow(xy_points[i].second - current_odom.second, 2));
            if (distance < min_distance) {
                min_distance = distance;
                potential_closest_index = i;
            }
        }
        if (potential_closest_index >= 0) {
            bool increasing_distance = false;
            int increasing_count = 0;
            const int increasing_threshold = 10; // 设置一个阈值，例如10或20

            for (int i = potential_closest_index + 1; i < xy_points.size(); ++i) {
                double distance = std::sqrt(std::pow(xy_points[i].first - current_odom.first, 2) +
                                            std::pow(xy_points[i].second - current_odom.second, 2));
                if (distance > min_distance) {
                    increasing_distance = true;
                    increasing_count++;
                    if (increasing_count >= increasing_threshold) {
                        break;
                    }
                } else {
                    // 如果发现更小的距离，则更新最小距离和索引
                    min_distance = distance;
                    potential_closest_index = i;
                    increasing_distance = false;
                    increasing_count = 0;
                }
            }
        }
    }else{
        int search_range = 10;
        int start_index = std::max(0, last_closest_index - search_range);
        int end_index = std::min(static_cast<int>(xy_points.size()) - 1, last_closest_index + search_range);
        
        for (int i = start_index; i <= end_index; ++i) {
            double distance = std::sqrt(std::pow(xy_points[i].first - current_odom.first, 2) +
                                        std::pow(xy_points[i].second - current_odom.second, 2));
            if (distance < min_distance) {
                min_distance = distance;
                potential_closest_index = i;
            }
        }

        if (potential_closest_index >= 0) {
            bool increasing_distance = false;
            int increasing_count = 0;
            const int increasing_threshold = 10; // 设置一个阈值，例如10或20

            for (int i = potential_closest_index + 1; i < xy_points.size(); ++i) {
                double distance = std::sqrt(std::pow(xy_points[i].first - current_odom.first, 2) +
                                            std::pow(xy_points[i].second - current_odom.second, 2));
                if (distance > min_distance) {
                    increasing_distance = true;
                    increasing_count++;
                    if (increasing_count >= increasing_threshold) {
                        break;
                    }
                } else {
                    // 如果发现更小的距离，则更新最小距离和索引
                    min_distance = distance;
                    potential_closest_index = i;
                    increasing_distance = false;
                    increasing_count = 0;
                }
            }
        }
    }

    // 更新最近点索引
    last_closest_index = potential_closest_index;
    closest_point_ = xy_points[last_closest_index];
    projection_point = CalculateProjectionPoint(current_odom, closest_point_, xy_points);
    PointAndIndex result;
    result.index = last_closest_index;
    result.x = projection_point.first;
    result.y = projection_point.second;


    return result;

}

std::pair<double, double> Match_Projection_Point::VectorFromPointToPoint(const std::pair<double, double>& pointA, const std::pair<double, double>& pointB) {
    return {pointB.first - pointA.first, pointB.second - pointA.second};
}

double Match_Projection_Point::DistanceBetweenPoints(const std::pair<double, double>& pointA, const std::pair<double, double>& pointB) {
    return std::sqrt(std::pow(pointB.first - pointA.first, 2) + std::pow(pointB.second - pointA.second, 2));
}

// 函数计算投影点
std::pair<double, double> Match_Projection_Point::CalculateProjectionPoint(const std::pair<double, double>& current_odom, 
                                                   const std::pair<double, double>& closest_point, 
                                                   const std::vector<std::pair<double, double>>& xy_points) {
    // 计算从当前位置到最近路径点的向量
    std::pair<double, double> vector_to_closest = VectorFromPointToPoint(current_odom, closest_point);
    double distance_to_closest = DistanceBetweenPoints(current_odom, closest_point);

    // 如果路径点只有一个，则最近点就是投影点
    if (xy_points.size() == 1) {
        return closest_point;
    }

    // 计算最近路径点前后的路径点
    std::pair<double, double> previous_point = (last_closest_index > 0) ? xy_points[last_closest_index - 1] : closest_point;
    std::pair<double, double> next_point = (last_closest_index < static_cast<int>(xy_points.size()) - 1) ? xy_points[last_closest_index + 1] : closest_point;

    // 计算路径切线向量
    std::pair<double, double> tangent_vector = VectorFromPointToPoint(previous_point, next_point);
    double tangent_length = DistanceBetweenPoints(previous_point, next_point);

    // 计算从最近点到路径切线的向量
    std::pair<double, double> vector_to_tangent = VectorFromPointToPoint(closest_point, previous_point);
    double dot_product = vector_to_closest.first * tangent_vector.first + vector_to_closest.second * tangent_vector.second;
    double scalar_projection = dot_product / tangent_length;

    // 计算投影点
    std::pair<double, double> projection_point;
    if (scalar_projection >= 0 && scalar_projection <= tangent_length) {
        projection_point.first = previous_point.first + scalar_projection * tangent_vector.first / tangent_length;
        projection_point.second = previous_point.second + scalar_projection * tangent_vector.second / tangent_length;
    } else {
        // 如果投影点在路径切线之外，则投影点就是最近点
        projection_point = closest_point;
    }

    return projection_point;
}

bool Match_Projection_Point::ComputePathProfileWithCache(
    const std::vector<std::pair<double, double>> &xy_points, CachedPathProfile &cache) {

    cache.headings.clear();
    cache.kappas.clear();
    cache.dkappas.clear();
    cache.accumulated_s.clear();

    if (xy_points.size() < 2)
    {
        ROS_ERROR("xy_points.size() ER!!!");
        return false;
    }
    std::vector<double> dxs, dys, y_over_s_first_derivatives, x_over_s_first_derivatives;
    std::vector<double> y_over_s_second_derivatives, x_over_s_second_derivatives;


    // Get finite difference approximated dx and dy for heading and kappa calculation
    std::size_t points_size = xy_points.size();
    // std::cout << "xy_points.size():" << xy_points.size() << std::endl;
    for (std::size_t i = 0; i < points_size; ++i)
    {
        double x_delta = 0.0;
        double y_delta = 0.0;
        if (i == 0)
        {
            x_delta = (xy_points[i + 1].first - xy_points[i].first);
            y_delta = (xy_points[i + 1].second - xy_points[i].second);
        }
        else if (i == points_size - 1)
        {
            x_delta = (xy_points[i].first - xy_points[i - 1].first);
            y_delta = (xy_points[i].second - xy_points[i - 1].second);
        }
        else
        {
            x_delta = 0.5 * (xy_points[i + 1].first - xy_points[i - 1].first);
            y_delta = 0.5 * (xy_points[i + 1].second - xy_points[i - 1].second);
        }
        dxs.push_back(x_delta);
        dys.push_back(y_delta);
    }
    // std::cout << "dxs.size():" << dxs.size() << std::endl;

    // Heading calculation
    for (std::size_t i = 0; i < points_size; ++i)
    {
        cache.headings.push_back(std::atan2(dys[i], dxs[i]));
    }

    // Get linear interpolated s for dkappa calculation
    double distance = 0.0;
    cache.accumulated_s.push_back(distance);
    double fx = xy_points[0].first;
    double fy = xy_points[0].second;
    double nx = 0.0;
    double ny = 0.0;
    for (std::size_t i = 1; i < points_size; ++i)
    {
        nx = xy_points[i].first;
        ny = xy_points[i].second;
        double end_segment_s = std::sqrt((fx - nx) * (fx - nx) + (fy - ny) * (fy - ny));
        cache.accumulated_s.push_back(end_segment_s + distance);
        distance += end_segment_s;
        fx = nx;
        fy = ny;
    }

    // Get finite difference approximated first derivative of y and x respective
    // to s for kappa calculation
    for (std::size_t i = 0; i < points_size; ++i)
    {
        double xds = 0.0;
        double yds = 0.0;
        if (i == 0)
        {
            xds = (xy_points[i + 1].first - xy_points[i].first) / (cache.accumulated_s.at(i + 1) - cache.accumulated_s.at(i));
            yds = (xy_points[i + 1].second - xy_points[i].second) / (cache.accumulated_s.at(i + 1) - cache.accumulated_s.at(i));
        }
        else if (i == points_size - 1)
        {
            xds = (xy_points[i].first - xy_points[i - 1].first) / (cache.accumulated_s.at(i) - cache.accumulated_s.at(i - 1));
            yds = (xy_points[i].second - xy_points[i - 1].second) / (cache.accumulated_s.at(i) - cache.accumulated_s.at(i - 1));
        }
        else
        {
            xds = (xy_points[i + 1].first - xy_points[i - 1].first) / (cache.accumulated_s.at(i + 1) - cache.accumulated_s.at(i - 1));
            yds = (xy_points[i + 1].second - xy_points[i - 1].second) / (cache.accumulated_s.at(i + 1) - cache.accumulated_s.at(i - 1));
        }
        x_over_s_first_derivatives.push_back(xds);
        y_over_s_first_derivatives.push_back(yds);
    }

    // Get finite difference approximated second derivative of y and x respective
    // to s for kappa calculation
    for (std::size_t i = 0; i < points_size; ++i)
    {
        double xdds = 0.0;
        double ydds = 0.0;
        if (i == 0)
        {
            xdds = (x_over_s_first_derivatives[i + 1] - x_over_s_first_derivatives[i]) /
                    (cache.accumulated_s.at(i + 1) - cache.accumulated_s.at(i));
            ydds = (y_over_s_first_derivatives[i + 1] - y_over_s_first_derivatives[i]) /
                    (cache.accumulated_s.at(i + 1) - cache.accumulated_s.at(i));
        }
        else if (i == points_size - 1)
        {
            xdds = (x_over_s_first_derivatives[i] - x_over_s_first_derivatives[i - 1]) /
                    (cache.accumulated_s.at(i) - cache.accumulated_s.at(i - 1));
            ydds = (y_over_s_first_derivatives[i] - y_over_s_first_derivatives[i - 1]) /
                    (cache.accumulated_s.at(i) - cache.accumulated_s.at(i - 1));
        }
        else
        {
            xdds = (x_over_s_first_derivatives[i + 1] - x_over_s_first_derivatives[i - 1]) /
                    (cache.accumulated_s.at(i + 1) - cache.accumulated_s.at(i - 1));
            ydds = (y_over_s_first_derivatives[i + 1] - y_over_s_first_derivatives[i - 1]) /
                    (cache.accumulated_s.at(i + 1) - cache.accumulated_s.at(i - 1));
        }
        x_over_s_second_derivatives.push_back(xdds);
        y_over_s_second_derivatives.push_back(ydds);
    }

    for (std::size_t i = 0; i < points_size; ++i)
    {
        double xds = x_over_s_first_derivatives[i];
        double yds = y_over_s_first_derivatives[i];
        double xdds = x_over_s_second_derivatives[i];
        double ydds = y_over_s_second_derivatives[i];

        double kappa = (xds * ydds - yds * xdds) / (std::sqrt(xds * xds + yds * yds) * (xds * xds + yds * yds) + 1e-6);
        cache.kappas.push_back(kappa);
    }

    // Dkappa calculation
    for (std::size_t i = 0; i < points_size; ++i)
    {
        double dkappa = 0.0;
        if (i == 0)
        {
            dkappa = (cache.kappas.at(i + 1) - cache.kappas.at(i)) / (cache.accumulated_s.at(i + 1) - cache.accumulated_s.at(i));
        }
        else if (i == points_size - 1)
        {
            dkappa = (cache.kappas.at(i) - cache.kappas.at(i - 1)) / (cache.accumulated_s.at(i) - cache.accumulated_s.at(i - 1));
        }
        else
        {
            dkappa = (cache.kappas.at(i + 1) - cache.kappas.at(i - 1)) / (cache.accumulated_s.at(i + 1) - cache.accumulated_s.at(i - 1));
        }
        cache.dkappas.push_back(dkappa);
    }
    return true;
}